#include "motor.h"

#define motor_offset		(92)
#define motor_offset3		(13)

motor_data_t motor_data[] = {
	{.dir = 1, .offset = motor_offset},{.dir = 1, .offset = motor_offset},{.dir = 1, .offset = motor_offset3},
	{.dir = 1, .offset = motor_offset},{.dir = 1, .offset = motor_offset},{.dir = 1, .offset = motor_offset3},
	{.dir = 1, .offset = motor_offset},{.dir = 1, .offset = motor_offset},{.dir = 1, .offset = motor_offset3},
	
	{.dir = -1, .offset = 88},{.dir = -1, .offset = 88},{.dir = -1, .offset = motor_offset3},
	{.dir = -1, .offset = 88},{.dir = -1, .offset = 88},{.dir = -1, .offset = motor_offset3},
	{.dir = -1, .offset = 88},{.dir = -1, .offset = 88},{.dir = -1, .offset = motor_offset3},
};

uint8_t motor_num = sizeof(motor_data) / sizeof(motor_data_t);

motor_tx_t motor_tx = {
	.tx_satae = tx_free,
	.mode = ctrl_angle,
};
/**
 * @brief  电机初始化
 * @param  
 * @retval 无
 */
void motor_init(void)
{
	for (uint8_t i=0; i < motor_num; i++) {
		motor_data[i].angle = 0;
	}
}

/**
 * @brief  电机控制延时
 * @param  
 * @retval 无
 */
void motor_delay(void)
{
	uint16_t cnt = 0x0f;
	
	while (cnt--) {
		__NOP();
	}
}

/**
 * @brief  电机发送数据
 * @param  
 * @retval 无
 */
void motor_tx_data(motor_tx_t* motor_tx, uint8_t num)
{
	uint8_t tx_angle = 0;
	
	if (num > motor_num) {
		return;
	}
	
	/* 限幅 */
	if (motor_data[num].offset > motor_angle_max) {
		motor_data[num].offset = motor_angle_max;
	}
	
	/* 计算夹角 */
	tx_angle = motor_data[num].angle + motor_data[num].offset;
	
	/* 判断方向 */
	if (motor_data[num].dir < 0) {
		tx_angle = motor_angle_max - tx_angle;
	}
	
	servo_c(num+1, tx_angle, 0, 1);
	
	motor_tx->txing_num = num + 1;
}

/**
* @brief  电机中断服务函数
 * @param  
 * @retval 无
 */
void motor_it_handler(void)
{
	motor_delay();
	if (motor_tx.txing_num < motor_num) {
		motor_tx_data(&motor_tx, motor_tx.txing_num);
	} else {
		motor_tx.tx_satae = tx_free;
	}
}

/**
 * @brief  控制id
 * @param  
 * @retval 无
 */
void motor_id_task(void)
{
	if (motor_tx.set_id > 240) {
		return ;
	}
	
	servo_id(motor_tx.set_id);
	vTaskDelay(500);
	
	servo_c(motor_tx.set_id, 0, 0, 1);
	vTaskDelay(1000);
	
	servo_c(motor_tx.set_id, 90, 0, 1);
	vTaskDelay(500);
}

/**
 * @brief  控制角度
 * @param  
 * @retval 无
 */
void motor_angle_task(void)
{
	if (motor_tx.tx_satae == tx_bus) {
		return;
	}
	
	motor_tx_data(&motor_tx, 0);
	motor_tx.tx_satae = tx_bus;
}

/**
* @brief  电机控制任务
 * @param  
 * @retval 无
 */
void motor_task_handler(void)
{
	switch(motor_tx.mode) {
		case ctrl_id: {
			motor_id_task();
			break;
		}
		
		case ctrl_angle: {
			motor_angle_task();
			break;
		}
	}
}

/**
 * @brief  控制电机
 * @param  
 * @retval 无
 */
void motor_set_angle(uint8_t id, int16_t angle)
{
	if (id > motor_num) {
		return ;
	}
	
	motor_data[id-1].angle = angle;
}
/******************* (C) COPYRIGHT 2025 FUYEBING*****END OF FILE************/
